
#include <boost/lexical_cast.hpp>

#include "Motion.h"

using namespace MMM;

Motion::Motion(const std::string &name, ModelPtr orginalModel, ModelPtr processedModel, ModelProcessorPtr modelProcessor, const std::string &originFilePath) :
    name(name),
    originFilePath(originFilePath),
    originalModel(orginalModel),
    processedModel(processedModel),
    modelProcessor(modelProcessor)
{
    if (this->name.empty()) throw Exception::MMMException("The motion name should not be an empty string!");
}

MotionPtr Motion::clone() {
    std::string name = this->name;
    MotionPtr clonedMotion(new Motion(name, originalModel ? originalModel->clone() : nullptr, processedModel ? processedModel->clone() : nullptr, modelProcessor, originFilePath));
    for (const auto &sensor : sensorData) clonedMotion->addSensor(sensor.second->clone()); // no exceptionHandling, because there should occure no Exception!
    return clonedMotion;
}

SensorPtr Motion::getSensorByName(const std::string &name) {
    return sensorData[name];
}

SensorPtr Motion::getSensorByType(const std::string &type) {
    for (const auto &sensor : sensorData) {
        if (sensor.second->getType() == type) return sensor.second;
    }
    return nullptr;
}


SensorList Motion::getSensorsByType(const std::string &type) {
    SensorList sensors;
    for (const auto &sensor : sensorData) {
        if (sensor.second->getType() == type) sensors.push_back(sensor.second);
    }
    return sensors;
}

bool Motion::hasSensor() {
    return sensorData.size() > 0;
}

bool Motion::hasSensor(const std::string &type) {
    return getSensorByType(type) != nullptr;
}

std::string Motion::getName() {
    return name;
}

std::string Motion::getOriginFilePath() {
    std::cout << originFilePath << std::endl;
    return originFilePath;
}

void Motion::setName(const std::string& name) {
    if (!name.empty()) this->name = name;
    else throw Exception::MMMException("Could not set name of motion " + this->name + ", because the new name is an empty string!");
}

std::map<std::string, SensorPtr> Motion::getSensorData() {
    return sensorData;
}

std::vector<SensorPtr> Motion::getPrioritySortedSensorData() {
    std::vector<SensorPtr> sortedSensors;
    for (const auto &sensor : sensorData) {
      sortedSensors.push_back(sensor.second);
    }
    std::stable_sort(sortedSensors.begin(), sortedSensors.end(), [] (SensorPtr p1, SensorPtr p2) { return p1->getPriority() > p2->getPriority();});
    return sortedSensors;
}

ModelPtr Motion::getModel(bool processed) {
    return processed ? processedModel : originalModel;
}

ModelProcessorPtr Motion::getModelProcessor() {
    return modelProcessor;
}

std::map<std::string, SensorMeasurementPtr> Motion::getAllMeasurementsForTimeStep(float timestep) {
    std::map<std::string, SensorMeasurementPtr> measurements;
    for (const auto &sensor : sensorData) {
        SensorMeasurementPtr measurement = sensor.second->getMeasurement(timestep);
        if (measurement) measurements[sensor.first] = measurement;
    }
    return measurements;
}

MotionPtr Motion::getSegmentMotion(float startTimestep, float endTimestep, bool changeTimestep) {
    return getSegmentMotion(startTimestep, endTimestep, name + "_segmented_" + XML::toString(startTimestep) + "f-" + XML::toString(endTimestep) + "f", changeTimestep);
}

MotionPtr Motion::getSegmentMotion(float startTimestep, float endTimestep, std::string segmentMotionName, bool changeTimestep) {
    if (startTimestep > endTimestep) {
        MMM_ERROR << "startTimestep needs to be bigger than endTimestep" << std::endl;
        return nullptr;
    }
    MotionPtr segmentMotion(new Motion(segmentMotionName, originalModel ? originalModel->clone() : nullptr, processedModel ? processedModel->clone() : nullptr, modelProcessor, originFilePath));
    for (const auto &sensor : sensorData) {
        SensorPtr segmentSensor = sensor.second->getSegmentSensor(startTimestep, endTimestep, changeTimestep);
        if (segmentSensor->getTimesteps().size() != 0) segmentMotion->addSensor(segmentSensor); // no exceptionHandling, because there should occure no Exception!
        else  MMM_INFO << "Ignoring sensor '" << segmentSensor->getName() << "' by segmenting, because the sensor has no corresponding measurements." << std::endl;
    }
    if (segmentMotion->sensorData.size() == 0) MMM_INFO << "Segmented motion does not contain any sensor." << std::endl;
    return segmentMotion;
}

void Motion::addSensor(SensorPtr sensor, float delta) {
    if (!sensor) throw Exception::MMMException("Sensor is null!");
    if (getSensor(sensor)) throw Exception::MMMException("Sensor is already contained in motion!");
    if (!sensor->checkModel(processedModel)) throw Exception::MMMException("Sensor not suited for the model of the motion!");
    if (sensor->getName().empty()) {
        AddingByType:
        if (!addSensor(sensor->getType(), sensor, delta)) {
            int i = 2;
            while (!addSensor(sensor->getType() + boost::lexical_cast<std::string>(i), sensor, delta)) i++;
        }
    } else {
        if (!addSensor(sensor->getName(), sensor, delta)) {
            MMM_INFO << "Already found a sensor with name '" << sensor->getName() << "'!"
                     << " Will add this sensor with '" << sensor->getType() << "' as part of his name." << std::endl;
            goto AddingByType;
        }
    }
}

bool Motion::addSensor(std::string name, SensorPtr sensor, float delta) {
    if (sensorData.find(name) == sensorData.end()) {
        sensor->setUniqueName(name);
        if (delta == 0.0f) sensorData[name] = sensor;
        else {
            float minTimestepSensor = sensor->getMinTimestep();
            float otherDelta = minTimestepSensor + delta;
            if (otherDelta > 0.0f) {
                sensor->shiftMeasurements(delta);
                sensorData[name] = sensor;
            } else {
                for (const auto &s : sensorData) {
                    s.second->shiftMeasurements(-1 * otherDelta);
                }
                sensor->shiftMeasurements(-1 * minTimestepSensor);
                sensorData[name] = sensor;
            }
        }
        return true;
    }
    else return false;
}

SensorPtr Motion::getSensor(SensorPtr s) {
    for (const auto &sensor : sensorData) {
        if(s->equalsConfiguration(sensor.second)) return s;
    }
    return nullptr;
}

MotionPtr Motion::join(MotionPtr motion1, MotionPtr motion2, std::string name) {
    if (!motion1->processedModel) return nullptr;
    std::string joinedMotionName = name.empty() ? motion1->name : name;
    MotionPtr joinedMotion(new Motion(joinedMotionName, motion1->originalModel->clone(), motion1->processedModel->clone(), motion1->modelProcessor));
    std::vector<SensorPtr> sensorCollection;
    for (const auto &sensor : motion1->sensorData) { // Collect Sensors from motion1
        sensorCollection.push_back(sensor.second);
    }
    for (const auto &sensor : motion2->sensorData) { // Collect Sensors from motion2, join if needed
        bool added = false;
        for (auto it = sensorCollection.begin(); it != sensorCollection.end(); ++it) {
            if (sensor.second->equalsConfiguration(*it)) {
                SensorPtr joinedSensor = Sensor::join(sensor.second, *it);
                if (joinedSensor) {
                    sensorCollection.erase(it);
                    sensorCollection.push_back(joinedSensor);
                    added = true;
                    break;
                }
                else return nullptr;
            }
        }
        if (!added) sensorCollection.push_back(sensor.second);
    }
    for (auto sensor : sensorCollection) { // Try Add found Sensors
        try {
            joinedMotion->addSensor(sensor->clone());
        } catch (Exception::MMMException& e) {
            MMM_ERROR << "Error when adding sensors from joined motion! " << e.what() << std::endl;
            return nullptr;
        }
    }
    return joinedMotion;
}

float Motion::getMaxTimestep() {
    if (sensorData.size() == 0) return 0.0f;
    float max = 0.0f;
    for (const auto &sensor : sensorData) {
        float maxT = sensor.second->getMaxTimestep();
        if (maxT > max) max = maxT;
    }
    return max;
}

float Motion::getMinTimestep() {
    if (sensorData.size() == 0) return 0.0f;
    float min = std::numeric_limits<float>::max();
    for (const auto &sensor : sensorData) {
        float minT = sensor.second->getMinTimestep();
        if (minT < min) min = minT;
    }
    return min;
}

float Motion::getCommonMinTimestep(const std::set<std::string> &ignoreSensorNames) {
    float max = 0.0f;
    for (const auto &sensor : sensorData) {
        if (ignoreSensorNames.find(sensor.second->getUniqueName()) != ignoreSensorNames.end()) continue;
        float minT = sensor.second->getMinTimestep();
        if (minT > max) max = minT;
    }
    return max;
}

float Motion::getCommonMaxTimestep(const std::set<std::string> &ignoreSensorNames) {
    float min = std::numeric_limits<float>::max();
    for (const auto &sensor : sensorData) {
        if (ignoreSensorNames.find(sensor.second->getUniqueName()) != ignoreSensorNames.end()) continue;
        float maxT = sensor.second->getMaxTimestep();
        if (maxT < min) min = maxT;
    }
    if (min > std::numeric_limits<float>::max() - 1.0f) return 0.0f;
    else return min;
}

void Motion::extendKinematic() {
    SensorList sensors = getSensorsByType("Kinematic");
    sensors.push_back(getSensorByType("ModelPose"));
    SensorList extendSensors;
    std::set<std::string> extendSensorNames;
    for (SensorPtr sensor : sensors) {
        if (sensor->getTimesteps().size() == 1) {
            extendSensors.push_back(sensor);
            extendSensorNames.insert(sensor->getUniqueName());
        }
    }
    if (extendSensors.size() > 0) {
        float minTimestep = getCommonMinTimestep(extendSensorNames);
        float maxTimestep = getCommonMaxTimestep(extendSensorNames);
        for (SensorPtr sensor : extendSensors)
            sensor->extend(minTimestep, maxTimestep);
    }
}

void Motion::synchronizeSensorMeasurements(float timeFrequency) {
    if (timeFrequency < 0.001) return;

    // erase non-interpolatable sensors
    for(auto it = sensorData.begin(); it != sensorData.end();) {
        if (!it->second->isInterpolatable()) {
            MMM_INFO << "Erasing sensor '" << it->first << "' from the motion, because it is not interpolatable." << std::endl;
            it = sensorData.erase(it);
        } else {
            it++;
        }
    }

    extendKinematic();

    float minTimestep = getCommonMinTimestep();
    float maxTimestep = getCommonMaxTimestep();
    for (const auto &sensor : sensorData) {
        sensor.second->synchronizeSensorMeasurements(timeFrequency, minTimestep, maxTimestep);
    }
}

bool Motion::isSynchronized() {
    std::vector<float> timesteps;
    bool initialized = false;
    for (auto sensor : sensorData) {
        if (!initialized) {
            timesteps = sensor.second->getTimesteps();
            initialized = true;
        }
        else if (timesteps != sensor.second->getTimesteps()) return false;
    }
    // test if difference between timesteps is overall the same
    if (timesteps.size() > 2) {
        float timestepDiff = timesteps.at(1) - timesteps.at(0);
        for (unsigned int i = 2; i < timesteps.size(); i++) {
            if (abs(timestepDiff - (timesteps.at(i) - timesteps.at(i - 1))) > 0.0001) return false;
        }
    }
    return true;
}

std::set<std::string> Motion::getSensorTypes() {
    std::set<std::string> sensorTypes;
    for (const auto &data : sensorData) {
        sensorTypes.insert(data.second->getType());
    }
    return sensorTypes;
}

MotionList Motion::replaceMotion(MotionPtr motion, MotionList motions) {
    for (auto & i : motions) {
        if (i->getName() == motion->getName()) {
            i = motion;
            break;
        }
    }
    return motions;
}

MotionList Motion::replaceAddMotion(MotionPtr motion, MotionList motions) {
    for (unsigned int i = 0; i < motions.size(); i++) {
        if (motions[i]->getName() == motion->getName()) {
            motions[i] = motion;
            return motions;
        }
    }
    motions.push_back(motion);
    return motions;
}

MotionPtr Motion::getMotion(MMM::MotionList motions, const std::string &motionName) {
    MMM::MotionPtr motion;
    if (motions.size() == 0) throw Exception::MMMException("No motions found in motion list!");
    else if (motionName.empty()) {
        if (motions.size() != 1) throw Exception::MMMException("Motion name should not be empty, if the motion file contains more than one motion!");
        else return motions[0];
    } else {
        for (MMM::MotionPtr m : motions) {
            if (m->getName() == motionName) motion = m;
        }
        if (!motion) throw Exception::MMMException("No motion with name '" + motionName + "' found in motion list!");
    }
    return motion;
}

std::tuple<float, float> Motion::calculateMinMaxTimesteps(MMM::MotionList motions) {
    float min = std::numeric_limits<float>::max();
    float max = 0.0f;

    for (MotionPtr motion : motions) {
        float maxMotionTimestep = motion->getMaxTimestep();
        if (max < maxMotionTimestep) max = maxMotionTimestep;

        float minMotionTimestep = motion->getMinTimestep();
        if (min > minMotionTimestep) min = minMotionTimestep;
    }

    if (min < max + 0.00001) return std::tuple<float, float>(min, max);
    else return std::tuple<float, float>(0.0f, 0.0f);
}
